#include <iostream>
#include <cmath>
#include <list>
#include <cppunit/CompilerOutputter.h>
#include <cppunit/extensions/TestFactoryRegistry.h>
#include <cppunit/ui/text/TestRunner.h>
#include <boost/numeric/mtl/mtl.hpp>
#include <CGAL/Random.h>

#include "fmm.h"
#include "isoline.h"
#include "geodesics.h"
#include "randompoint.h"
#include "gcvt.h"
#include "backtracking.h"

using namespace std;
void test1(char ** argv)
{
	Mesh mesh;
	OpenMesh::IO::Options ropt, wopt;

	if (! OpenMesh::IO::read_mesh(mesh, argv[1], ropt))
	{
		std::cerr<<"Error loading mesh from file "<<argv[1]<<endl;
	}
	//cout<<mesh.n_vertices()<<endl;
	//unsigned n_vertices = mesh.n_vertices();
	//Mesh::VertexIter v_it, v_end (mesh.vertices_end());
	
	//Mesh::VertexHandle v_handle = mesh.vertex_handle(0);
	//cout<<mesh.point (v_handle)<<endl;

	Mesh::VertexIter v_it, v_end (mesh.vertices_end());
	for (v_it = mesh.vertices_begin(); v_it != v_end; ++v_it)
	{
		if (! mesh.is_manifold(v_it) )
		{
			cerr<<"Not manifold"<<endl;
			return ;
		}
	}
	cerr<<"The model passes manifold test"<<endl;

	std::vector<Mesh::VertexHandle> boundary;
	boundary.push_back(mesh.vertex_handle(0));
	mesh.data(mesh.vertex_handle(0)).set_distance(0);
	fast_marching (mesh, boundary);

	unsigned n_vertices = mesh.n_vertices();
	//Mesh::VertexIter v_it, v_end (mesh.vertices_end());
	for (v_it = mesh.vertices_begin(); v_it != v_end; ++v_it)
	{
		cout<<mesh.data(v_it).get_distance()<<endl;
	}
}
void test2 ()
{
	//flip(double AB, double BC, double AC, double AD, double BD);
	//cout<< flip(sqrt(2.0f), 1.0f, 1, 1, 1) <<endl;

	
	//flip(double AB, double BC, double AC, double AD, double BD);
	//cout<< flip(sqrt(2.0f), 1.0f, 1, 1, 1) <<endl;
	//cout<<flip(sqrt(3.0f), 2, 1, 1, 1)<<endl;
}
void test3(char ** argv)
{
	Mesh mesh;
	OpenMesh::IO::Options ropt, wopt;

	if (! OpenMesh::IO::read_mesh(mesh, argv[1], ropt))
	{
		std::cerr<<"Error loading mesh from file "<<argv[1]<<endl;
	}

	Mesh::VertexVertexIter vv_it;
	vv_it= mesh.vv_iter( mesh.vertex_handle(0) );
	
	for (; vv_it; --vv_it)
	{
		cout<<vv_it.handle().idx()<<endl;
	}
	cout<<"Break"<<endl;
	cout<<vv_it.handle().idx()<<endl;
	--vv_it;
	cout<<vv_it.handle().idx()<<endl;
	//std::vector<Mesh::VertexHandle> boundary;
	//boundary.push_back(mesh.vertex_handle(0));
	//mesh.data(mesh.vertex_handle(0)).set_distance(0);
	//fast_marching (mesh, boundary);
}
void test4(char ** argv)
{
	//Test the function of flip;

	//int obtuse_process (Mesh & mesh, Mesh::VertexHandle va, Mesh::VertexHandle vb, Mesh::VertexHandle vc, double & tc)
	Mesh mesh;
	OpenMesh::IO::Options ropt, wopt;

	if (! OpenMesh::IO::read_mesh(mesh, argv[1], ropt))
	{
		std::cerr<<"Error loading mesh from file "<<argv[1]<<endl;
	}


	mesh.data(mesh.vertex_handle(9)).set_state(ALIVE);
	mesh.data(mesh.vertex_handle(9)).set_distance(1);

//	mesh.data(mesh.vertex_handle(10)).set_state(ALIVE);
//	mesh.data(mesh.vertex_handle(10)).set_distance(0);

	mesh.data(mesh.vertex_handle(3)).set_state(ALIVE);
	mesh.data(mesh.vertex_handle(3)).set_distance(2);

	double tc;
	//obtuse_local_solver (mesh, mesh.vertex_handle(9), mesh.vertex_handle(10), mesh.vertex_handle(16), 1, tc);
	cout<<tc<<endl;


}

void test5_draw_isoline(char ** argv)
{
	Mesh mesh;
	OpenMesh::IO::Options ropt, wopt;

	if (! OpenMesh::IO::read_mesh(mesh, argv[1], ropt))
	{
		std::cerr<<"Error loading mesh from file "<<argv[1]<<endl;
	}
	//cout<<mesh.n_vertices()<<endl;
	//unsigned n_vertices = mesh.n_vertices();
	//Mesh::VertexIter v_it, v_end (mesh.vertices_end());
	
	//Mesh::VertexHandle v_handle = mesh.vertex_handle(0);
	//cout<<mesh.point (v_handle)<<endl;

	Mesh::VertexIter v_it, v_end (mesh.vertices_end());
	for (v_it = mesh.vertices_begin(); v_it != v_end; ++v_it)
	{
		if (! mesh.is_manifold(v_it) )
		{
			cerr<<"Not manifold"<<endl;
			return ;
		}
	}
	cerr<<"The model passes manifold test"<<endl;

	std::vector<Mesh::VertexHandle> initial;
	//unsigned start_vertex_index = 50; // sphere /*Work badly*/
	unsigned start_vertex_index ; // sphere /*Work badly*/
	//unsigned start_vertex_index = 0;
	//unsigned start_vertex_index = 1086;   // Plane5
	//unsigned start_vertex_index = 383; // The cube1 start index
	Mesh::VertexHandle v_handle;

	start_vertex_index = 46;
	v_handle = mesh.vertex_handle(start_vertex_index); 
	mesh.data(mesh.vertex_handle(start_vertex_index)).set_distance(0);
	mesh.data(v_handle).set_state(ALIVE);
	initial.push_back(v_handle);

	start_vertex_index = 60;
	v_handle = mesh.vertex_handle(start_vertex_index); 
	mesh.data(mesh.vertex_handle(start_vertex_index)).set_distance(5);
	mesh.data(v_handle).set_state(ALIVE);
	initial.push_back(v_handle);

	fast_marching (mesh, initial);

	/*
	for (v_it = mesh.vertices_begin(); v_it != v_end; ++v_it)
	{
		mesh.data(v_it).set_state(FAR);
	}
	start_vertex_index = 60;
	v_handle = mesh.vertex_handle(start_vertex_index); 
	mesh.data(mesh.vertex_handle(start_vertex_index)).set_distance(0);
	mesh.data(v_handle).set_state(ALIVE);
	initial.clear();
	initial.push_back(v_handle);
	fast_marching (mesh, initial);
	*/


	

	double max_dis = 0;
	for (v_it = mesh.vertices_begin(); v_it != v_end; ++v_it)
	{
		if (max_dis < mesh.data(v_it).get_distance())
		{
			max_dis = mesh.data(v_it).get_distance();
		}
	}

	int n_lines = 100;
	double step = max_dis / (n_lines + 1);
	
	double isovalue = 0;
	std::vector<Mesh::Point> line;
	for (int i = 0; i < n_lines; ++i)
	{
		isovalue +=step;		
		draw_one_isoline(mesh, isovalue, line);
	}
	
	assert (line.size() % 2 == 0); 
	//outputoff(line);
	//outputobj(line);
	//generate_merged_mesh_isoline(mesh, line);
	generate_seperate_mesh_isoline(mesh, line);

	int count = 0;
	for (Mesh::VertexIter v_it = mesh.vertices_begin(); v_it != mesh.vertices_end(); ++v_it)
	{
		std::cerr<<count++<<' '<<mesh.data(v_it).get_distance()<<std::endl;
	}

}



void test7_draw_geodesics_face_point (char ** argv)
{
	Mesh mesh;
	OpenMesh::IO::Options ropt, wopt;

	if (! OpenMesh::IO::read_mesh(mesh, argv[1], ropt))
	{
		std::cerr<<"Error loading mesh from file "<<argv[1]<<std::endl;
	}
	//cout<<mesh.n_vertices()<<endl;
	//unsigned n_vertices = mesh.n_vertices();
	//Mesh::VertexIter v_it, v_end (mesh.vertices_end());
	
	//Mesh::VertexHandle v_handle = mesh.vertex_handle(0);
	//cout<<mesh.point (v_handle)<<endl;

	Mesh::VertexIter v_it, v_end (mesh.vertices_end());
	for (v_it = mesh.vertices_begin(); v_it != v_end; ++v_it)
	{
		if (! mesh.is_manifold(v_it) )
		{
			std::cerr<<"Not manifold"<<std::endl;
			return ;
		}
	}
	std::cerr<<"The model passes manifold test"<<std::endl;

	//unsigned start_index = 404; //sphere
	unsigned start_index = 40; //sphere
	std::vector<Mesh::VertexHandle> initial;
	initial.push_back(mesh.vertex_handle(start_index));
	Mesh::Point end = mesh.point(mesh.vertex_handle(start_index));
	mesh.data(mesh.vertex_handle(start_index)).set_distance(0);
	fast_marching (mesh, initial);

	std::vector<Mesh::FaceHandle> end_faces;
	for (Mesh::VertexFaceIter vf_it = mesh.vf_iter(mesh.vertex_handle(start_index)); vf_it; ++vf_it)
	{
		end_faces.push_back(vf_it.handle());
		//std::cout<<vf_it.handle().idx()<<std::endl;
	}
	

	unsigned center_face_idx = 500;
	//unsigned center_face_idx = 50;

	Mesh::FaceHandle start_face = mesh.face_handle(center_face_idx);
	Mesh::Point start(0,0,0);
	for (Mesh::FaceVertexIter fv_it  = mesh.fv_iter(start_face); fv_it; ++fv_it)
	{
		
		start [0] +=mesh.point(fv_it)[0];
		start [1] +=mesh.point(fv_it)[1];
		start [2] +=mesh.point(fv_it)[2];
	}
	start [0] /= 3.0f;
	start [1] /= 3.0f;
	start [2] /= 3.0f;

	std::list<Mesh::Point> path;
	Mesh::HalfedgeHandle start_on_heh; 
	Mesh::VertexHandle start_point_handle;
	draw_geodesics(mesh,  start, start_point_handle, start_face, start_on_heh,  FACE_POINT, end, end_faces, 0.001, path);
	generate_seperate_mesh_geodesics(mesh, path);
}


int main(int arc, char ** argv)
{

	//test1(argv);
	//test2();
	//test3(argv);

	//test4(argv);
	//test5_draw_isoline(argv);
	test7_draw_geodesics_face_point(argv);
	//test8_draw_geodesics_edge_point(argv);
	//test9_draw_geodesics_vertex_point(argv);
	return 0;
}


